package com.zonerobotics.zBotAPI.PollEngine;

import java.nio.ByteBuffer;
/*
 * There is a simple prototocol that reads open memory on the zbot robot
 * you simply pass it the header, then pass it the offset of the memory you want to read
 * and the amount of bytes you want to read. 
 * 
 * Since control of more than one bot is not allowed from one device (yet?) all of the 
 * variables are static. Once we do our first read they are written with the values from 
 * the device, if we change them they will go out in the next write.
 * 
 */
import java.nio.ByteOrder;

//import android.util.Log;

/* THIS IS THE STRUCT ON THE ZBOT94
 * typedef struct glbVars
{
	// READ VARIABLES ********************************************
	 
	// enviromental
	u32 status;			// current status including errors
	u32 update;			// zbot requests you update something 
	
	// adc conversions
	u32 ADCIN1;			// 
	u32 ADCIN2;			// 
	u32 FQ1;			// captures the frequency on this pin

	// ReadWrite Variables, we will read often, write rarely

	// enviromental
	u32 gpioVal;	  	// live values of gpio
	u32 seconds;	  	// current seconds 
	u32 milliseconds;  	// current milliseconds
	u32 i2cRXBuff;		// last rx command from i2c

	// some space for a few future use vars
	u32 FutureR1;			//	future use ? 
	u32 FutureR2;			//	future use ? 
	
	// WRITE VARIABLES (CAN BE READ ALSO) ***********************

	u32 enabled;		// what perih are enabled
	u32 gpioIO;		  	// which gpio are input and which are output 
	// periph specific 
	u32 PWM1FQ;		   	// frequency of pwm
	u8 PWM1CH1Duty;	   	// 
	u8 PWM1CH2Duty;
	u8 PWM1CH3Duty;
	u8 PWM1CH4Duty;
	u32 i2cTXBuff;	   	// data sent to i2c bus
	u32 DAC1;			// dac1 
	u32 DAC2;			// dac2 
	// some space for a few future use vars
	u32 FutureW1;		//	future use ? 
	u32 FutureW2;		//	future use ? 

	// internal should not be read or written!
	u32 dataState;		// current state of data tx/rx in main

} globalVars, *pgloabalVars;
 */

/**
 * @author auction
 *
 */
public class ZPacket {

    static public enum protoCommand{
		read,
		write,
		readwrite
	}
	


	/**************************************************
	 * PACKET STRUCTURE
	 * NAME		DESC			BYTES		DEF VALUE
	 * ID		id of bot			1			0xBB
	 * TXN		transaction			1			variable
	 * CMD		command 			1			0x00read, 0x01write, 0x02r/w
	 * FLG		err flags			1			not really used
	 * SIZE		size of payload		2			size of payload
	 * PAYLOAD	data in/out			var			variable, normally straight out of mem (see struct above)
	 * CRC		crc entire packet	4			entire packet except crc is crc'ed
	 * 
	 */



	// local copy of the header (id,txn,cmd,flags,size(2byte))
	static byte[] header = new byte[]{(byte) (0xBB & 0xFF),0x00,0x00,0x00,0x00,0x00};
	// local copy of crc
	byte[] crc = new byte[4]; 
	/**
	 * place to begin reading from 
	 */
	private short readStartIndex = -1;
	/**
	 * how many to read
	 */
	private short readLength = -1;
	/**
	 * where to start writing
	 */
	private short writeStartIndex = -1;
	/**
	 * length in ints to write, 4 bytes
	 */
	private short writeLength = -1;
	
	// local valid var, true by default, something must mark it invalid
	private boolean valid = true;
	
	
	/**
	 * this is where the variables the user changes 
	 * get stored as bytes to get written to the device
	 * memory. It will also be the raw payload coming back of
	 * readable variables to write to the bot.
	 */
	private byte[] varPayload=null;
	
	public int fullSizeInBytes = 0;
	
	// check to see if packet is valid before building etc
	public boolean isValid()
	{
		if(!valid)
		{
			return false;
		}
		// todo check all vars to make sure ok
		return true;
	}
	
	public void setVarPayload(byte[] rawData, short position)
	{
		this.varPayload = rawData;
	}

	/**
	 * this converts bytes to a packet, it is expected that
	 * we have all of the bytes we need, this does no buffering
	 * txrxpacket makes sure we have all of the expected bytes
	 * before calling this
	 * @param rawBytes
	 * raw bytes from stream
	 * @return
	 */
	public int fromBytes(byte[] rawBytes)
	{
		int cmd = 0;
		boolean lockOn = false;
		
		ByteBuffer bb = null;
		
		// wrap the heap and check 
		bb = ByteBuffer.wrap(rawBytes);
		
		bb.order(ByteOrder.LITTLE_ENDIAN);
		
		try
		{
			while(!lockOn)
			{
				int id = bb.get();
				// look for the first one, -69 in unsigned is 0xbb
				while(bb.hasRemaining() && (id != -69))
				{
					id = bb.get();
				}
				if(!bb.hasRemaining())
				{
					valid = false;
					return -1;
				}
				
				bb.get();
				cmd = bb.get();
				// if its a valid command, 2 is the only supported for now
				if(cmd == 2)
				{
					lockOn = true;
				}
			}
			
	
			bb.get();
			
			// get the amount of bytes left
			readLength = bb.getShort();

			// go ahead and fill out the payload
			setVarPayload(new byte[readLength]);
			bb.get(getVarPayload(), 0, readLength);
			
			// Increment the txn
			header[1]++;

		}
		catch(Exception ex)
		{
			//if(D) Log.e(TAG, "exception while parsing packet" + ex.getMessage());
			valid = false;
			return -1;
		}
		return 0;
	}

	/**
	 * This function converts the packet tobytes. It is assumed that the header
	 * already contains the type of message and this switches off that. if the header
	 * is a read or readwrite command startIndex and rwLength are used
	 * @return
	 */
	public byte[] toBytes()
	{
		byte[] payload = null;
		header[2] = 0x02; // always read write ! 
		if(getVarPayload() != null)
		{
			writeLength = (short) getVarPayload().length;
		}
		switch(header[2])
		{		
			case 0x02: // read write message is exactly the same for now, except we will get resp
				// these must be made before hand
				if(writeStartIndex == -1 || writeLength == -1 || readStartIndex == -1 || readLength == -1)
				{
					return null; 
				}
				// if its a write message get our payload ready
				// header + start + len + payload + crc
				payload = ByteBuffer.allocate(header.length + 8 + getVarPayload().length + 4)
						.order(ByteOrder.LITTLE_ENDIAN)
						.put(header, 0, header.length)
						.putShort(readStartIndex) // read the normal amount
						.putShort(readLength) // read the normal min len
						// position to start writing and count
						.putShort(writeStartIndex)
						.putShort(writeLength)
						// begin payload
						.put(getVarPayload())
						// end 
						.putInt(-1) // crc nothing for now
						.array();
				// fix up size of payload in header TODO better later
				payload[4] = (byte)(payload.length - header.length - 4);
				payload[5] = 0x00;
				break;
			default:
				break;
		}
		fullSizeInBytes = payload.length;
		return payload;
	}

	public ZPacket build()
	{
		// if the gui sent some new stuff update
		if(writeStartIndex == -1 || writeLength == -1 || readStartIndex == -1 || readLength == -1)
		{
			return null; 
		}
		return this;
	}

	// getters and setters 
	
	public ZPacket setID(byte ID)
	{
		header[0] = ID;
		return this;
	}

	public ZPacket setTXN(byte TXN)
	{
		header[1] = TXN;
		return this;
	}
	
	public byte getTXN()
	{
		return header[1];
	}

	public ZPacket setCMD(protoCommand cmd)
	{
		switch(cmd)
		{
		case read:
			header[2] = 0x00;
			break;
		case write:
			header[2] = 0x01;
			break;
		case readwrite:
			header[2] = 0x02;
			break;
		default:
			header[2] = (byte) 0xFF;
		}
		return this;
	}


	public byte[] getHeader() {
		return header;
	}

	public ZPacket setHeader(byte[] header) {
		this.header = header; 
		return this;
	}


	/**
	 * this will set the video flag so the message will get 
	 * data from the video buffer not the status vars
	 */
	public ZPacket setVideoFlag()
	{
		// todo set official flags in define
		header[3] |= 0x01; 
		return this;
	}
	
	public ZPacket clearVideoFlag()
	{
		// todo set official flags in define
		// clear first bit
		header[3] &= 0x7E;
		return this;
	}
	
	/**
	 * this returns true if video flag is set
	 * @return
	 * true if video flag is set
	 */
	public boolean hasVideoData()
	{
		return ((header[3] & 0x01) > 0);
	}

	/**
	 * returns the length of the variable payload
	 * this is very useful for checking data validity
	 * @return
	 */
	public int getVarPayloadLen()
	{
		if(varPayload == null)
			return 0;
		return varPayload.length;
	}
	
	public byte[] getVarPayload() {
		if(varPayload == null)
		{
			return new byte[0];
		}
		return varPayload;
	}

	public void setVarPayload(byte[] varPayload) {
		this.varPayload = varPayload;
	}
	
	public short getReadStartIndex() {
		return readStartIndex;
	}

	public ZPacket setReadStartIndex(int readStartIndex) {
		this.readStartIndex = (short) readStartIndex;
		return this;
	}

	public short getReadLength() {
		return readLength;
	}

	public ZPacket setReadLength(int readLength) {
		this.readLength = (short) readLength;
		return this;
	}

	public short getWriteStartIndex() {
		return writeStartIndex;
	}
	
	public ZPacket setWriteStartIndex(int writeStartIndex) {
		this.writeStartIndex = (short) writeStartIndex;
		return this;
	}

	public short getWriteLength() {
		return writeLength;
	}

	public ZPacket setWriteLength(int writeLength) {
		this.writeLength = (short) writeLength;
		return this;
	}
}
